#include <stdio.h>
#include <stdlib.h>
#include "path.h"
#include "datamanger.h"
#include "floyd.h"


//     0
// 3       2
//     1
// d`  1   2   3   4
// x`  2   1   4   3
// n`  4   3   1   2
// b`  3   4   2   1
//   FORWARD = 0x01,     //前进
//   BACKWARD = 0x02,     //向后
//   TURNRIGHT = 0x03,   //原地右转
//   TURNLEFT = 0x04,     //原地左转
const uint8_t dirtection_buff[4][4] = {  { 1, 2, 3, 4},
                                    { 2, 1, 4, 3},
                                    { 4, 3, 1, 2},
                                    { 3, 4, 2, 1}};     //方向查询真值表    列：当前机器人方向，行：下一个点的方向  DIRECTION_E



STATION_T *start_station;   //机器人出发点，也就是充电房
STATION_T station[STATION_NUMBERS +1];      //所有停靠点

TASK_LIST_T *task_list_head = NULL;    //链表头，也就是下一个巡检点，到达之后释放，指向下一个点
TASK_LIST_T *task_list_tail = NULL;    //链表尾

TASK_LIST_T *path_list_head = NULL;     //路径链表， 巡检点之间的路径
TASK_LIST_T *path_list_tail = NULL;

NAVI_PROCESS_T navigation;     //机器人当前时刻的位置


void insert_a_path_station(int sta);

/**
 * @brief 初始化全局地图，建立各个停靠点之间的联系
 * 
 */
void global_map_create(void)
{
    /*       东                             西                              南                              北*/
    station[0].p_direction[P_EAST] = NULL;           station[0].p_direction[P_WEST] = NULL;           station[0].p_direction[P_SOUTH] = NULL;              station[0].p_direction[P_NORTH] = &station[1];    
    station[1].p_direction[P_EAST] = &station[2];    station[1].p_direction[P_WEST] = NULL;           station[1].p_direction[P_SOUTH] = &station[0];       station[1].p_direction[P_NORTH] = &station[26]; 
    station[2].p_direction[P_EAST] = &station[3];    station[2].p_direction[P_WEST] = &station[1];    station[2].p_direction[P_SOUTH] = NULL;              station[2].p_direction[P_NORTH] = NULL; 
    station[3].p_direction[P_EAST] = &station[4];    station[3].p_direction[P_WEST] = &station[2];    station[3].p_direction[P_SOUTH] = NULL;              station[3].p_direction[P_NORTH] = NULL;
    station[4].p_direction[P_EAST] = &station[5];    station[4].p_direction[P_WEST] = &station[3];    station[4].p_direction[P_SOUTH] = NULL;              station[4].p_direction[P_NORTH] = NULL;
    station[5].p_direction[P_EAST] = &station[6];    station[5].p_direction[P_WEST] = &station[4];    station[5].p_direction[P_SOUTH] = NULL;              station[5].p_direction[P_NORTH] = NULL;
    station[6].p_direction[P_EAST] = &station[7];    station[6].p_direction[P_WEST] = &station[5];    station[6].p_direction[P_SOUTH] = NULL;              station[6].p_direction[P_NORTH] = NULL;
    station[7].p_direction[P_EAST] = &station[8];    station[7].p_direction[P_WEST] = &station[6];    station[7].p_direction[P_SOUTH] = NULL;              station[7].p_direction[P_NORTH] = NULL;
    station[8].p_direction[P_EAST] = &station[9];    station[8].p_direction[P_WEST] = &station[7];    station[8].p_direction[P_SOUTH] = NULL;              station[8].p_direction[P_NORTH] = NULL;
    station[9].p_direction[P_EAST] = &station[10];   station[9].p_direction[P_WEST] = &station[8];    station[9].p_direction[P_SOUTH] = NULL;              station[9].p_direction[P_NORTH] = NULL;
    station[10].p_direction[P_EAST] = &station[11];  station[10].p_direction[P_WEST] = &station[9];   station[10].p_direction[P_SOUTH] = NULL;             station[10].p_direction[P_NORTH] = NULL;
    station[11].p_direction[P_EAST] = &station[12];  station[11].p_direction[P_WEST] = &station[10];  station[11].p_direction[P_SOUTH] = NULL;             station[11].p_direction[P_NORTH] = NULL;
    station[12].p_direction[P_EAST] = &station[13];  station[12].p_direction[P_WEST] = &station[11];  station[12].p_direction[P_SOUTH] = NULL;             station[12].p_direction[P_NORTH] = NULL;

    station[13].p_direction[P_EAST] = &station[12];  station[13].p_direction[P_WEST] = &station[14];  station[13].p_direction[P_SOUTH] = NULL;              station[13].p_direction[P_NORTH] = NULL;
    station[14].p_direction[P_EAST] = &station[13];  station[14].p_direction[P_WEST] = &station[15];  station[14].p_direction[P_SOUTH] = NULL;              station[14].p_direction[P_NORTH] = NULL;
    station[15].p_direction[P_EAST] = &station[14];  station[15].p_direction[P_WEST] = &station[16];  station[15].p_direction[P_SOUTH] = NULL;              station[15].p_direction[P_NORTH] = NULL;
    station[16].p_direction[P_EAST] = &station[15];  station[16].p_direction[P_WEST] = &station[17];  station[16].p_direction[P_SOUTH] = NULL;              station[16].p_direction[P_NORTH] = NULL;
    station[17].p_direction[P_EAST] = &station[16];  station[17].p_direction[P_WEST] = &station[18];  station[17].p_direction[P_SOUTH] = NULL;              station[17].p_direction[P_NORTH] = NULL;
    station[18].p_direction[P_EAST] = &station[17];  station[18].p_direction[P_WEST] = &station[19];  station[18].p_direction[P_SOUTH] = NULL;              station[18].p_direction[P_NORTH] = NULL;
    station[19].p_direction[P_EAST] = &station[18];  station[19].p_direction[P_WEST] = &station[20];  station[19].p_direction[P_SOUTH] = NULL;              station[19].p_direction[P_NORTH] = NULL;
    station[20].p_direction[P_EAST] = &station[19];  station[20].p_direction[P_WEST] = &station[21];  station[20].p_direction[P_SOUTH] = NULL;              station[20].p_direction[P_NORTH] = NULL;
    station[21].p_direction[P_EAST] = &station[10];  station[21].p_direction[P_WEST] = &station[22];  station[21].p_direction[P_SOUTH] = NULL;              station[21].p_direction[P_NORTH] = NULL;
    station[22].p_direction[P_EAST] = &station[21];  station[22].p_direction[P_WEST] = &station[23];  station[22].p_direction[P_SOUTH] = NULL;              station[22].p_direction[P_NORTH] = NULL;
    station[23].p_direction[P_EAST] = &station[22];  station[23].p_direction[P_WEST] = &station[24];  station[23].p_direction[P_SOUTH] = NULL;              station[23].p_direction[P_NORTH] = NULL;
    station[24].p_direction[P_EAST] = &station[23];  station[24].p_direction[P_WEST] = &station[25];  station[24].p_direction[P_SOUTH] = NULL;              station[24].p_direction[P_NORTH] = NULL;
    station[25].p_direction[P_EAST] = &station[24];  station[25].p_direction[P_WEST] = &station[26];  station[25].p_direction[P_SOUTH] = NULL;              station[25].p_direction[P_NORTH] = NULL;
    station[26].p_direction[P_EAST] = &station[25];  station[26].p_direction[P_WEST] = NULL;          station[26].p_direction[P_SOUTH] = &station[1];       station[26].p_direction[P_NORTH] = &station[27];


    station[27].p_direction[P_EAST] = &station[28];  station[27].p_direction[P_WEST] = NULL;          station[27].p_direction[P_SOUTH] = &station[26];      station[27].p_direction[P_NORTH] = &station[45];
    station[28].p_direction[P_EAST] = &station[29];  station[28].p_direction[P_WEST] = &station[27];  station[28].p_direction[P_SOUTH] = NULL;              station[28].p_direction[P_NORTH] = NULL;
    station[29].p_direction[P_EAST] = &station[30];  station[29].p_direction[P_WEST] = &station[28];  station[29].p_direction[P_SOUTH] = NULL;              station[29].p_direction[P_NORTH] = NULL;
    station[30].p_direction[P_EAST] = &station[31];  station[30].p_direction[P_WEST] = &station[29];  station[30].p_direction[P_SOUTH] = NULL;              station[30].p_direction[P_NORTH] = NULL;
    station[31].p_direction[P_EAST] = &station[32];  station[31].p_direction[P_WEST] = &station[30];  station[31].p_direction[P_SOUTH] = NULL;              station[31].p_direction[P_NORTH] = NULL;
    station[32].p_direction[P_EAST] = &station[33];  station[32].p_direction[P_WEST] = &station[31];  station[32].p_direction[P_SOUTH] = NULL;              station[32].p_direction[P_NORTH] = NULL;
    station[33].p_direction[P_EAST] = &station[34];  station[33].p_direction[P_WEST] = &station[32];  station[33].p_direction[P_SOUTH] = NULL;              station[33].p_direction[P_NORTH] = NULL;
    station[34].p_direction[P_EAST] = &station[35];  station[34].p_direction[P_WEST] = &station[33];  station[34].p_direction[P_SOUTH] = NULL;              station[34].p_direction[P_NORTH] = NULL;
    station[35].p_direction[P_EAST] = &station[36];  station[35].p_direction[P_WEST] = &station[34];  station[35].p_direction[P_SOUTH] = NULL;              station[35].p_direction[P_NORTH] = NULL;
    station[36].p_direction[P_EAST] = &station[37];  station[36].p_direction[P_WEST] = &station[35];  station[36].p_direction[P_SOUTH] = NULL;              station[36].p_direction[P_NORTH] = NULL;

    station[37].p_direction[P_EAST] = NULL;          station[37].p_direction[P_WEST] = NULL;          station[37].p_direction[P_SOUTH] = &station[36];      station[37].p_direction[P_NORTH] = &station[38];
    station[38].p_direction[P_EAST] = &station[37];  station[38].p_direction[P_WEST] = &station[39];  station[38].p_direction[P_SOUTH] = NULL;              station[38].p_direction[P_NORTH] = NULL;
    station[39].p_direction[P_EAST] = &station[38];  station[39].p_direction[P_WEST] = &station[40];  station[39].p_direction[P_SOUTH] = NULL;              station[39].p_direction[P_NORTH] = NULL;
    station[40].p_direction[P_EAST] = &station[39];  station[40].p_direction[P_WEST] = &station[41];  station[40].p_direction[P_SOUTH] = NULL;              station[40].p_direction[P_NORTH] = NULL;
    station[41].p_direction[P_EAST] = &station[40];  station[41].p_direction[P_WEST] = &station[42];  station[41].p_direction[P_SOUTH] = NULL;              station[41].p_direction[P_NORTH] = NULL;
    station[42].p_direction[P_EAST] = &station[41];  station[42].p_direction[P_WEST] = &station[43];  station[42].p_direction[P_SOUTH] = NULL;              station[42].p_direction[P_NORTH] = NULL;
    station[43].p_direction[P_EAST] = &station[42];  station[43].p_direction[P_WEST] = &station[44];  station[43].p_direction[P_SOUTH] = NULL;              station[43].p_direction[P_NORTH] = NULL;
    station[44].p_direction[P_EAST] = &station[43];  station[44].p_direction[P_WEST] = &station[45];  station[44].p_direction[P_SOUTH] = NULL;              station[44].p_direction[P_NORTH] = NULL;
    station[45].p_direction[P_EAST] = &station[44];  station[45].p_direction[P_WEST] = &station[46];  station[45].p_direction[P_SOUTH] = NULL;              station[45].p_direction[P_NORTH] = NULL;
    station[46].p_direction[P_EAST] = &station[45];  station[46].p_direction[P_WEST] = NULL;          station[46].p_direction[P_SOUTH] = &station[27];      station[46].p_direction[P_NORTH] = &station[47];


    station[47].p_direction[P_EAST] = NULL;          station[47].p_direction[P_WEST] = NULL;          station[47].p_direction[P_SOUTH] = &station[46];      station[47].p_direction[P_NORTH] = &station[48];
    station[48].p_direction[P_EAST] = &station[50];  station[48].p_direction[P_WEST] = &station[47];  station[48].p_direction[P_SOUTH] = NULL;              station[48].p_direction[P_NORTH] = &station[49];
    station[49].p_direction[P_EAST] = NULL;          station[49].p_direction[P_WEST] = NULL;          station[49].p_direction[P_SOUTH] = &station[48];      station[49].p_direction[P_NORTH] = NULL;
    station[50].p_direction[P_EAST] = &station[52];  station[50].p_direction[P_WEST] = &station[48];  station[50].p_direction[P_SOUTH] = NULL;              station[50].p_direction[P_NORTH] = &station[51];
    station[51].p_direction[P_EAST] = NULL;          station[51].p_direction[P_WEST] = NULL;          station[51].p_direction[P_SOUTH] = &station[50];      station[51].p_direction[P_NORTH] = NULL;
    station[52].p_direction[P_EAST] = NULL;          station[52].p_direction[P_WEST] = &station[50];  station[52].p_direction[P_SOUTH] = NULL;              station[52].p_direction[P_NORTH] = NULL;

    //距离数据填充
    /*       东                             西                              南                              北*/
    station[0].distance[P_EAST] = 0;        station[0].distance[P_WEST] = 0;        station[0].distance[P_SOUTH] = 0;           station[0].distance[P_NORTH] = 560;    
    station[1].distance[P_EAST] = 1400;     station[1].distance[P_WEST] = 0;        station[1].distance[P_SOUTH] = 560;         station[1].distance[P_NORTH] = 1144; 
    station[2].distance[P_EAST] = 310;      station[2].distance[P_WEST] = 1400;     station[2].distance[P_SOUTH] = 0;           station[2].distance[P_NORTH] = 0; 
    station[3].distance[P_EAST] = 330;      station[3].distance[P_WEST] = 310;      station[3].distance[P_SOUTH] = 0;           station[3].distance[P_NORTH] = 0;
    station[4].distance[P_EAST] = 600;      station[4].distance[P_WEST] = 330;      station[4].distance[P_SOUTH] = 0;           station[4].distance[P_NORTH] = 0;
    station[5].distance[P_EAST] = 490;      station[5].distance[P_WEST] = 600;      station[5].distance[P_SOUTH] = 0;           station[5].distance[P_NORTH] = 0;
    station[6].distance[P_EAST] = 290;      station[6].distance[P_WEST] = 490;      station[6].distance[P_SOUTH] = 0;           station[6].distance[P_NORTH] = 0;
    station[7].distance[P_EAST] = 540;      station[7].distance[P_WEST] = 290;      station[7].distance[P_SOUTH] = 0;           station[7].distance[P_NORTH] = 0;
    station[8].distance[P_EAST] = 520;      station[8].distance[P_WEST] = 540;      station[8].distance[P_SOUTH] = 0;           station[8].distance[P_NORTH] = 0;
    station[9].distance[P_EAST] = 390;      station[9].distance[P_WEST] = 520;      station[9].distance[P_SOUTH] = 0;           station[9].distance[P_NORTH] = 0;
    station[10].distance[P_EAST] = 2620;    station[10].distance[P_WEST] = 390;     station[10].distance[P_SOUTH] = 0;          station[10].distance[P_NORTH] = 0;
    station[11].distance[P_EAST] = 1906;    station[11].distance[P_WEST] = 2620;    station[11].distance[P_SOUTH] = 0;          station[11].distance[P_NORTH] = 0;
    station[12].distance[P_EAST] = 1000;    station[12].distance[P_WEST] = 1906;    station[12].distance[P_SOUTH] = 0;          station[12].distance[P_NORTH] = 0;

    station[13].distance[P_EAST] = 1000;    station[13].distance[P_WEST] = 934;     station[13].distance[P_SOUTH] = 0;          station[13].distance[P_NORTH] = 0;
    station[14].distance[P_EAST] = 934;     station[14].distance[P_WEST] = 940;     station[14].distance[P_SOUTH] = 0;          station[14].distance[P_NORTH] = 0;
    station[15].distance[P_EAST] = 940;     station[15].distance[P_WEST] = 990;     station[15].distance[P_SOUTH] = 0;          station[15].distance[P_NORTH] = 0;
    station[16].distance[P_EAST] = 990;     station[16].distance[P_WEST] = 1380;    station[16].distance[P_SOUTH] = 0;          station[16].distance[P_NORTH] = 0;
    station[17].distance[P_EAST] = 1380;    station[17].distance[P_WEST] = 630;     station[17].distance[P_SOUTH] = 0;          station[17].distance[P_NORTH] = 0;
    station[18].distance[P_EAST] = 630;     station[18].distance[P_WEST] = 760;     station[18].distance[P_SOUTH] = 0;          station[18].distance[P_NORTH] = 0;
    station[19].distance[P_EAST] = 760;     station[19].distance[P_WEST] = 540;     station[19].distance[P_SOUTH] = 0;          station[19].distance[P_NORTH] = 0;
    station[20].distance[P_EAST] = 540;     station[20].distance[P_WEST] = 350;     station[20].distance[P_SOUTH] = 0;          station[20].distance[P_NORTH] = 0;
    station[21].distance[P_EAST] = 350;     station[21].distance[P_WEST] = 430;     station[21].distance[P_SOUTH] = 0;          station[21].distance[P_NORTH] = 0;
    station[22].distance[P_EAST] = 430;     station[22].distance[P_WEST] = 630;     station[22].distance[P_SOUTH] = 0;          station[22].distance[P_NORTH] = 0;
    station[23].distance[P_EAST] = 630;     station[23].distance[P_WEST] = 580;     station[23].distance[P_SOUTH] = 0;          station[23].distance[P_NORTH] = 0;
    station[24].distance[P_EAST] = 580;     station[24].distance[P_WEST] = 640;     station[24].distance[P_SOUTH] = 0;          station[24].distance[P_NORTH] = 0;
    station[25].distance[P_EAST] = 640;     station[25].distance[P_WEST] = 470;     station[25].distance[P_SOUTH] = 0;          station[25].distance[P_NORTH] = 0;
    station[26].distance[P_EAST] = 470;     station[26].distance[P_WEST] = 0;       station[26].distance[P_SOUTH] = 1144;       station[26].distance[P_NORTH] = 1044;


    station[27].distance[P_EAST] = 1182;    station[27].distance[P_WEST] = 0;       station[27].distance[P_SOUTH] = 1044;       station[27].distance[P_NORTH] = 1608;
    station[28].distance[P_EAST] = 502;     station[28].distance[P_WEST] = 1182;    station[28].distance[P_SOUTH] = 0;          station[28].distance[P_NORTH] = 0;
    station[29].distance[P_EAST] = 481;     station[29].distance[P_WEST] = 502;     station[29].distance[P_SOUTH] = 0;          station[29].distance[P_NORTH] = 0;
    station[30].distance[P_EAST] = 827;     station[30].distance[P_WEST] = 481;     station[30].distance[P_SOUTH] = 0;          station[30].distance[P_NORTH] = 0;
    station[31].distance[P_EAST] = 498;     station[31].distance[P_WEST] = 827;     station[31].distance[P_SOUTH] = 0;          station[31].distance[P_NORTH] = 0;
    station[32].distance[P_EAST] = 485;     station[32].distance[P_WEST] = 498;     station[32].distance[P_SOUTH] = 0;          station[32].distance[P_NORTH] = 0;
    station[33].distance[P_EAST] = 1479;    station[33].distance[P_WEST] = 485;     station[33].distance[P_SOUTH] = 0;          station[33].distance[P_NORTH] = 0;
    station[34].distance[P_EAST] = 1809;    station[34].distance[P_WEST] = 1479;    station[34].distance[P_SOUTH] = 0;          station[34].distance[P_NORTH] = 0;
    station[35].distance[P_EAST] = 1778;    station[35].distance[P_WEST] = 1809;    station[35].distance[P_SOUTH] = 0;          station[35].distance[P_NORTH] = 0;
    station[36].distance[P_EAST] = 2573;    station[36].distance[P_WEST] = 1778;    station[36].distance[P_SOUTH] = 0;          station[36].distance[P_NORTH] = 0;

    station[37].distance[P_EAST] = 0;       station[37].distance[P_WEST] = 0;       station[37].distance[P_SOUTH] = 2573;              station[37].distance[P_NORTH] = 2510;
    station[38].distance[P_EAST] = 2510;    station[38].distance[P_WEST] = 1860;    station[38].distance[P_SOUTH] = 0;          station[38].distance[P_NORTH] = 0;
    station[39].distance[P_EAST] = 1860;    station[39].distance[P_WEST] = 3600;    station[39].distance[P_SOUTH] = 0;          station[39].distance[P_NORTH] = 0;
    station[40].distance[P_EAST] = 3600;    station[40].distance[P_WEST] = 275;     station[40].distance[P_SOUTH] = 0;          station[40].distance[P_NORTH] = 0;
    station[41].distance[P_EAST] = 275;     station[41].distance[P_WEST] = 790;     station[41].distance[P_SOUTH] = 0;          station[41].distance[P_NORTH] = 0;
    station[42].distance[P_EAST] = 790;     station[42].distance[P_WEST] = 841;     station[42].distance[P_SOUTH] = 0;          station[42].distance[P_NORTH] = 0;
    station[43].distance[P_EAST] = 841;     station[43].distance[P_WEST] = 311;     station[43].distance[P_SOUTH] = 0;          station[43].distance[P_NORTH] = 0;
    station[44].distance[P_EAST] = 311;     station[44].distance[P_WEST] = 759;     station[44].distance[P_SOUTH] = 0;          station[44].distance[P_NORTH] = 0;
    station[45].distance[P_EAST] = 759;     station[45].distance[P_WEST] = 620;     station[45].distance[P_SOUTH] = 0;          station[45].distance[P_NORTH] = 0;
    station[46].distance[P_EAST] = 620;     station[46].distance[P_WEST] = 0;       station[46].distance[P_SOUTH] = 1608;       station[46].distance[P_NORTH] = 2551;
  
    station[47].distance[P_EAST] = 0;       station[47].distance[P_WEST] = 0;       station[47].distance[P_SOUTH] = 2551;       station[47].distance[P_NORTH] = 1000;
    station[48].distance[P_EAST] = 1000;    station[48].distance[P_WEST] = 1000;    station[48].distance[P_SOUTH] = 0;          station[48].distance[P_NORTH] = 1000;
    station[49].distance[P_EAST] = 0;       station[49].distance[P_WEST] = 0;       station[49].distance[P_SOUTH] = 1000;       station[49].distance[P_NORTH] = 0;
    station[50].distance[P_EAST] = 2000;    station[50].distance[P_WEST] = 1000;    station[50].distance[P_SOUTH] = 0;          station[50].distance[P_NORTH] = 1000;
    station[51].distance[P_EAST] = 0;       station[51].distance[P_WEST] = 0;       station[51].distance[P_SOUTH] = 1000;       station[51].distance[P_NORTH] = 0;
    station[52].distance[P_EAST] = 0;       station[52].distance[P_WEST] = 2000;    station[52].distance[P_SOUTH] = 0;          station[52].distance[P_NORTH] = 0;


    for(int i = 0; i <= STATION_NUMBERS; i++)
    {
        station[i].this_station = i;
    }

    navigation.cur_station = 0;                 //默认值
    navigation.next_station = 0;
    navigation.distance = 0;
    navigation.robot_direction = P_NORTH;       //默认机器人朝向北
    navigation.run_direction = P_NORTH;

    // navigation.cur_station = 8;                 //默认值
    // navigation.next_station = 8;
    // navigation.distance = 0;
    // navigation.robot_direction = P_EAST;       //默认机器人朝向北
    // navigation.run_direction = P_WEST;

}

/**
 * @brief 打印每个station的数据
 * 
 */
void show_map(void)
{
    int i = 0;
    printf("----------Show map----------n");
    for( i = 0; i <= STATION_NUMBERS; i++)
    {
        printf("\t");
        if(station[i].p_direction[P_EAST] != NULL){
            printf("%d\n",station[i].p_direction[P_EAST]->this_station);
        }else{
            printf("N\n");
        }

        if(station[i].p_direction[P_NORTH] != NULL){
            printf("%d\t",station[i].p_direction[P_NORTH]->this_station);
        }else{
            printf("N\t");
        }
        printf("%d\t",i);

        if(station[i].p_direction[P_SOUTH] != NULL){
            printf("%d\n",station[i].p_direction[P_SOUTH]->this_station);
        }else{
            printf("N\n");
        }

        if(station[i].p_direction[P_WEST] != NULL){
            printf("\t%d\n",station[i].p_direction[P_WEST]->this_station);
        }else{
            printf("\tN\n");
        }
        printf("\n");
        //sleep(1);
    }

    printf("----------Map end----------\n");
}


/**
 * @brief 插入一个巡检任务，也就是添加一个待巡检点
 * 
 * @param sta ：待 巡检点号
 */
void insert_a_station(int sta)
{
    TASK_LIST_T *plist = task_list_head;    //获取头部指针
    TASK_LIST_T *pnew = NULL;
    if (plist == NULL)   //队列为空
    {
        pnew = (TASK_LIST_T *) malloc(sizeof(TASK_LIST_T));   
        pnew->station_index = sta;
        pnew->pnext = NULL;

        task_list_head = pnew;  //插入队列
        task_list_tail = pnew;
        return;
    }

    if(task_list_head->station_index > sta) //应该插入在头部
    {
        pnew = (TASK_LIST_T *) malloc(sizeof(TASK_LIST_T));   
        pnew->station_index = sta;
        pnew->pnext = task_list_head;
        task_list_head = pnew;
        return;
    }
    if(task_list_tail->station_index < sta) //应该插入在尾部
    {
        pnew = (TASK_LIST_T *) malloc(sizeof(TASK_LIST_T));   
        pnew->station_index = sta;
        pnew->pnext = NULL;
        task_list_tail->pnext = pnew;
        task_list_tail = pnew;
        return;
    }
    
    while(plist->pnext != NULL)  //查询插入位置
    {
        if((plist->pnext->station_index > sta) && (plist->station_index < sta))  //按顺序存入队列
        {
            pnew = (TASK_LIST_T *) malloc(sizeof(TASK_LIST_T));   
            pnew->station_index = sta;
            pnew->pnext = plist->pnext;
            plist->pnext = pnew;
            return;
        }
        plist = plist->pnext;
    }
    printf("insert_a_station error!!!\n");
}

/**
 * @brief 插入所有停靠点
 * 
 */
void insert_all_stations(void)
{
    int i;
    for(i = 0; i <= 52; i++)
    {
        if((i == 1) || (i == 26) || (i == 27) || (i == 46) || (i == 47) || (i == 48) || (i == 50))  continue;
        insert_a_station(i);
    }

}

/**
 * @brief 获取到下一个目标节点的路径
 * 
 * @return int 返回获取到的下一个目标节点，当队列中没有节点，返回-1
 */
int get_next_station(void)
{
    int sta;
    TASK_LIST_T *plist = task_list_head;    //获取头部指针

    if(plist == NULL)
    {
        return -1;
    }

    sta = plist->station_index;
    task_list_head = plist->pnext;
    free(plist);

    printf("%d--%d: ",navigation.cur_station, sta);
    plan_path(navigation.cur_station, sta, (void *)insert_a_path_station);  //规划当前点到目标点的路径
    printf("\n");

    return sta;

}

void clear_task_station(void)
{
    TASK_LIST_T *plist = task_list_head;    //获取头部指针

    while(plist != NULL)
    { 
        task_list_head = task_list_head->pnext;
        free(plist);
        plist = task_list_head;  
    }
}


/**
 * @brief 打印所有待巡检的停靠点
 * 
 */
void show_task_station(void)
{
    int sta;
    TASK_LIST_T *plist = task_list_head;    //获取头部指针
    printf("----------task stations----------\n");

    while(plist != NULL)
    {
        sta = plist->station_index;
        printf("%d->",sta);

        plist = plist->pnext;

    }
    printf("\n");
}



/*-------------路径-----------*/
/**
 * @brief 路径链表（按路线顺序插入）    ,不插入当前点，
 * 
 * @param sta ；路径点
 */
void insert_a_path_station(int sta)
{
    TASK_LIST_T *plist = NULL;    //获取头部指针
    if(sta > STATION_NUMBERS)       //输入点号有误
    {
        printf("insert a path station error!!!\n");
        return;
    }else{
        //printf("insert station: %d\n",sta);
    }

    plist = (TASK_LIST_T *) malloc(sizeof(TASK_LIST_T));         //创建新节点
    plist->station_index = sta;
    plist->pnext = NULL;
    if(path_list_head == NULL)
    {
        path_list_head = plist;
        path_list_tail = plist;
        return;
    }

    path_list_tail->pnext = plist;
    path_list_tail = plist;         //

}

/**
 * @brief 清除路径数据
 * 
 */
void clear_path_station(void)
{
    TASK_LIST_T *plist = path_list_head;    //获取头部指针

    while(plist != NULL)
    { 
        path_list_head = path_list_head->pnext;
        free(plist);
        plist = path_list_head;  
    }
}
/**
 * @brief Get the path station object
 *  
 * @return int 机器人到下一个点的动作, -1：表示当前点需要停靠
 */
ACTION_TYPE get_path_station(void)
{
    int i;
    int sta;
    TASK_LIST_T *plist = path_list_head;    //获取头部指针
    if(plist == NULL)
    {
        navigation.cur_station = navigation.next_station;   //清除
        //navigation.next_station = 0;
        navigation.distance = 0;
        return 0;
    }
    sta = plist->station_index;
    path_list_head = plist->pnext;
    free(plist);

    navigation.cur_station = navigation.next_station;
    navigation.next_station = sta;
    //printf("%d -> %d; dist: %d i = %d\n",navigation.cur_station,navigation.next_station, navigation.distance,i);

    for(i = 0; i < 4; i++)      //判断下一个点相对于当前点的方位
    {   
        if(station[navigation.cur_station].p_direction[i] == &station[navigation.next_station])      //是同一个点
        {
            navigation.distance = station[navigation.cur_station].distance[i];      //获取到对应方向点的距离
            //break; 
            printf("%d -> %d; dist: %d i = %d, act_type: %d\n",navigation.cur_station,navigation.next_station, navigation.distance,i, dirtection_buff[navigation.robot_direction][i]);

            return (ACTION_TYPE) (dirtection_buff[navigation.robot_direction][i]);
        }

    }
    return 0;
    // printf("%d -> %d; dist: %d i = %d\n",navigation.cur_station,navigation.next_station, navigation.distance,i);

    // return dirtection_buff[navigation.robot_direction][i];
}







